#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdint.h"
#include <stdarg.h>

// RT-Thread 头文件
#include <rtthread.h>
#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include <rtdevice.h>
#include <board.h>

#include "de_bug_mouse.h"
#include "rt_thread_demo.h"
#include "../hardware/Flight_control/fly_control.h"     // 飞控

// 这些设备必须初始化才能使用对应的串口输出
extern rt_device_t uart2_dev; 
extern rt_device_t uart3_dev;
extern rt_device_t uart4_dev;
extern rt_device_t uart5_dev;

#if LIBC_ENABLE == 0
// 串口输出函数
static void serial_putchar(int serial, char c) {
    if (serial == SERIAL1) {
        rt_kprintf("%c", c);
    } else {
        rt_device_t dev = RT_NULL;
        if      (serial == SERIAL2) dev = uart2_dev;
        else if (serial == SERIAL3) dev = uart3_dev;
        else if (serial == SERIAL4) dev = uart4_dev;
        else if (serial == SERIAL5) dev = uart5_dev;
        if (dev) rt_device_write(dev, 0, &c, 1);
        else {
            rt_kprintf("Error: Serial device %d not initialized.\n", serial);
        }
    }
}

// 输出整数
static void serial_putint(int serial, int num) {
    char buf[12];
    int i = 0, is_neg = 0;
    if (num < 0) {
        is_neg = 1;
        num = -num;
    }
    do {
        buf[i++] = num % 10 + '0';
        num /= 10;
    } while (num);
    if (is_neg) buf[i++] = '-';
    while (i--) serial_putchar(serial, buf[i]);
}

// 输出两位小数的浮点数
static void serial_putfloat(int serial, double f) {
    if (f < 0) {
        serial_putchar(serial, '-');
        f = -f;
    }
    int int_part  = (int)f;
    int frac_part = (int)((f - int_part) * 100 + 0.5); // 四舍五入
    serial_putint(serial, int_part);
    serial_putchar(serial, '.');
    if (frac_part < 10) serial_putchar(serial, '0');
    serial_putint(serial, frac_part);
}

/****************************** Serial_printf BEGIN ****************************
 * 函    数：Serial_printf
 * 功    能：格式化输出到指定串口
 * 支持格式：%f（浮点数）、%d（整数）、%c（字符）、%%（百分号）
 * 参    数：serial - 串口号
 *           fmt    - 格式化字符串
 * 返 回 值：无
 * 注意事项：无
 ******************************************************************************/
void Serial_printf(int serial, const char *fmt, ...) {
    va_list args;
    va_start(args, fmt);

    while (*fmt) {
        if (*fmt == '%' && *(fmt + 1) == 'f') {
            double f = va_arg(args, double);
            serial_putfloat(serial, f);
            fmt += 2;
        } else if (*fmt == '%' && *(fmt + 1) == 'd') {
            int d = va_arg(args, int);
            serial_putint(serial, d);
            fmt += 2;
        } else if (*fmt == '%' && *(fmt + 1) == 'c') {
            char c = (char)va_arg(args, int);
            serial_putchar(serial, c);
            fmt += 2;
        } else if (*fmt == '%' && *(fmt + 1) == '%') {
            serial_putchar(serial, '%');
            fmt += 2;
        } else {
            serial_putchar(serial, *fmt);
            ++fmt;
        }
    }

    va_end(args);
}
#else

 void Serial_printf(int serial, const char *fmt, ...) {
    char buf[128];
    va_list args;
    va_start(args, fmt);
    int len = vsnprintf(buf, sizeof(buf), fmt, args);
    va_end(args);

    rt_device_t dev = RT_NULL;
    if      (serial == SERIAL1) { rt_kprintf("%s", buf); return; }
    else if (serial == SERIAL2) dev = uart2_dev;
    else if (serial == SERIAL3) dev = uart3_dev;
    else if (serial == SERIAL4) dev = uart4_dev;
    else if (serial == SERIAL5) dev = uart5_dev;

    if (dev && len > 0)
        rt_device_write(dev, 0, buf, len);
    else if (!dev)
        rt_kprintf("Error: Serial device %d not initialized.\n", serial);
}
#endif 
/****************************** Serial_printf END ******************************/
